#pragma once

namespace Physics
{
	#define SCALAR_TINY ((float) 1.0e-6)
	#define SCALAR_HUGE ((float) 1.0e6)

	struct Transform3
	{
		enum Identity {IDENTITY};

		/// public access
		Vector3 Position;
		Matrix3 Orientation;

		/// default ctor does not initialise
		Transform3() {}
		Transform3(Identity) : Position(Vector3::Zero), Orientation(Quaternion::Identity) {}
		Transform3(const Vector3& vec, const Quaternion& mat) : Position(vec), Orientation(mat) {}
		Transform3(const Vector3& vec, const Matrix3& mat) : Position(vec), Orientation(mat) {}

		const Vector3& getPos() const { return Position; }
		const Vector3& getLeft() const { return Orientation[0]; }
		const Vector3& getUp() const { return Orientation[1]; }
		const Vector3& getLook() const { return Orientation[2]; }

		friend Transform3 operator*(const Transform3 &lhs, const Transform3 &rhs)
		{
			return Transform3(lhs.Position + lhs.Orientation * rhs.Position,
				lhs.Orientation * rhs.Orientation);
		}

		friend void MultTranform3(Transform3 &out, const Transform3 &lhs, const Transform3 &rhs)
		{
			out.Position = lhs.Position + lhs.Orientation * rhs.Position;
			out.Orientation = lhs.Orientation * rhs.Orientation;
		}
	};

	struct Transform3Rate
	{
		/// public access
		Vector3 Velocity;
		Vector3 AngVelocity;
		
		Transform3Rate() {}
		Transform3Rate(const Vector3 &vel, const Vector3 &angVel) : Velocity(vel), AngVelocity(angVel) {}

		void SetToZero() { Velocity = AngVelocity = Vector3::Zero; }
	};
	
	class Line
	{
	public:
		Line() {}
		Line(const Vector3 & origin, const Vector3 & dir) : mOrigin(origin), mDir(dir) {}

		Vector3 GetOrigin(float t) const {return mOrigin + mDir * t;}

		Vector3 mOrigin;
		Vector3 mDir;
	};

	/// A Ray is just a line that extends in the +ve direction
	class Ray
	{
	public:
		Ray() {}
		Ray(const Vector3 & origin, const Vector3 & dir) : mOrigin(origin), mDir(dir) {}

		Vector3 GetOrigin(float t) const {return mOrigin + mDir * t;}

		Vector3 mOrigin;
		Vector3 mDir;
	};
	struct Segment
	{
		Vector3 Origin;
		Vector3 Delta;

		Vector3 GetPoint(float t) const {return Origin + Delta * t;}
		Vector3 GetEnd() const {return Origin + Delta;}

		Segment() {}
		Segment(const Vector3 & origin, const Vector3 & delta) : Origin(origin), Delta(delta) {}
	};
};
